﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Timers;
using System.Windows.Forms;
using VBClass;

namespace wingbot
{
    public partial class mainForm : Form
    {
        //bool port_available = false;
        WingPort port = new WingPort();
        WingPort port2 = new WingPort(); //二号车
        Label[] Heights;
        TextBox[] Angles;
        public double WheelVel = 0.0;
        private object LastCommand;

        public bool IsTeamed { get { return (port.RobotState.team_info.enable>0)&&(port2.RobotState.team_info.enable>0); } }

        public mainForm()
        {
            InitializeComponent();
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            ActiveControl = panelCar;
            this.KeyPreview = true;//获取或设置一个值，该值指示在将键事件传递到具有焦点的控件前，窗体是否将接收此键事件
            ToggleControl("Control", false);    //默认禁用控制按键
            ArrayList ports = Transport.ListPorts();
            lbWheelVel.Text = WheelVel.ToString();
            Heights = new Label[4] { Height1, Height2, Height3, Height4 };
            Angles = new TextBox[4] { Angle1, Angle2, Angle3, Angle4 };

            if (ports.Count > 0)
            {
                foreach (string port in ports)
                {
                    cbSerials.Items.Add(port);
                }
                cbSerials.SelectedItem = cbSerials.Items[0];
            }
        }

        private void BtnOpenCOM_Click(object sender, EventArgs e)
        {
            if (btnOpenCOM.Text == "OpenCOM")
                port.Begin(cbSerials.Text, 921600);
            else
                port.Close();

            if (port.Ready)
            {
                ToggleControl("Control", true);
                port.BeginRead();
                port.RequestRobotState();
                btnOpenCOM.Text = "CloseCOM";
            }
            else
            {
                ToggleControl("Control", false);
                btnOpenCOM.Text = "OpenCOM";
            }
            ActiveControl = panelCar;
        }

        /// <summary>
        /// 切换控件是否可用
        /// </summary>
        /// <param name="tag">Tag的值</param>
        /// <param name="state">状态</param>
        private void ToggleControl(string tag, bool state)
        {
            foreach (Control c in this.Controls)
            {
                if (c is Button)
                {
                    if (c.Tag != null)
                    {
                        if (c.Tag.ToString() == tag)
                            c.Enabled = state;
                    }
                }
            }
        }

        private void BtnGetVersion_Click(object sender, EventArgs e)
        {
            port.RequestVersion();
            ActiveControl = panelCar;
        }

        private void BtnSetMotor_Click(object sender, EventArgs e)
        {
            port.EachMotor.WheelFL.VL = (short)numWheelFLMotorL.Value; //左前轮-左电机
            port.EachMotor.WheelFL.VR = (short)numWheelFLMotorR.Value; //左前轮-右电机
            port.EachMotor.WheelFR.VL = (short)numWheelFRMotorL.Value; //右前轮-左电机
            port.EachMotor.WheelFR.VR = (short)numWheelFRMotorR.Value; //右前轮-右电机

            port.EachMotor.WheelBL.VL = (short)numWheelBLMotorL.Value; //左后轮-左电机
            port.EachMotor.WheelBL.VR = (short)numWheelBLMotorR.Value; //左后轮-右电机
            port.EachMotor.WheelBR.VL = (short)numWheelBRMotorL.Value; //右后轮-左电机
            port.EachMotor.WheelBR.VR = (short)numWheelBRMotorR.Value; //右后轮-右电机

            port.SetEachMotor();
            ActiveControl = panelCar;
        }

        private void btnStop_Click(object sender, EventArgs e)
        {
            port.EachMotor.WheelFL.VL = 0;
            port.EachMotor.WheelFL.VR = 0;
            port.EachMotor.WheelFR.VL = 0;
            port.EachMotor.WheelFR.VR = 0;

            port.EachMotor.WheelBL.VL = 0;
            port.EachMotor.WheelBL.VR = 0;
            port.EachMotor.WheelBR.VL = 0;
            port.EachMotor.WheelBR.VR = 0;

            port.SetEachMotor();
            setRobotSpeed(0);
            ActiveControl = panelCar;
        }

        private void setRobotSpeed(short vel)
        {
            numWheelFLMotorL.Value = vel; //左前轮-左电机
            numWheelFLMotorR.Value = -vel; //左前轮-右电机
            numWheelFRMotorL.Value = -vel; //右前轮-左电机
            numWheelFRMotorR.Value = vel; //右前轮-右电机

            numWheelBLMotorL.Value = vel; //左后轮-左电机
            numWheelBLMotorR.Value = -vel; //左后轮-右电机
            numWheelBRMotorL.Value = -vel; //右后轮-左电机
            numWheelBRMotorR.Value = vel; //右后轮-右电机
        }

        private void btnSetForward_Click(object sender, EventArgs e)
        {
            setRobotSpeed(20);
            ActiveControl = panelCar;
        }

        private void btnSetBack_Click(object sender, EventArgs e)
        {
            setRobotSpeed(-20);
            ActiveControl = panelCar;
        }

        private void btnHydroStretch_Click(object sender, EventArgs e)
        {
            port.Arm.Couples.ArmL.Pump = 0x01;
            port.Arm.Couples.ArmL.Strech = 0x01;
            port.Arm.Couples.ArmL.DrawBack = 0x00;
            port.Arm.Couples.ArmR.Pump = 0x01;
            port.Arm.Couples.ArmR.Strech = 0x01;
            port.Arm.Couples.ArmR.DrawBack = 0x00;

            port.SetArms();
        }

        private void btnHydrauArmStop_Click(object sender, EventArgs e)
        {
            port.Arm.Couples.ArmL.Pump = 0x00;
            port.Arm.Couples.ArmR.Pump = 0x00;

            port.SetArms();
        }

        private void btnHydroBack_Click(object sender, EventArgs e)
        {
            port.Arm.Couples.ArmL.Pump = 0x01;
            port.Arm.Couples.ArmL.Strech = 0x00;
            port.Arm.Couples.ArmL.DrawBack = 0x01;
            port.Arm.Couples.ArmR.Pump = 0x01;
            port.Arm.Couples.ArmR.Strech = 0x00;
            port.Arm.Couples.ArmR.DrawBack = 0x01;

            port.SetArms();
            ActiveControl = panelCar;
        }

        private void mainForm_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == ' ')
            {
                btnStop_Click(sender,e);
            }
        }

        private void update_command(string cmd_name)
        {
            List<WingPort> ports = new List<WingPort> { port, port2 };
            double trans_angle = Convert.ToDouble(txtTransAngle.Text);
            short vel_y = Convert.ToInt16(WheelVel * Math.Sin(trans_angle/180*Math.PI) * 1000);
            short vel_x = Convert.ToInt16(WheelVel * Math.Cos(trans_angle/180*Math.PI) * 1000);
            int port_amount = IsTeamed ? 2 : 1;

            short get_vth(short vx)
            {
                short ret = Convert.ToInt16(vx / Convert.ToDouble(txtTurnRadius.Text));
                return Math.Abs(ret);
            }

            if (cmd_name == "btnMoveF")
            {
                for(int i=0;i<port_amount;i++)
                    ports[i].SetVelocity(Convert.ToInt16(WheelVel * 1000), 0, 0);
            }
            else if (cmd_name == "btnMoveB")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetVelocity(Convert.ToInt16(-WheelVel * 1000), 0, 0);
            }
            else if (cmd_name == "btnMoveFL")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetVelocity(vel_x, vel_y, 0);
            }
            else if (cmd_name == "btnMoveFR")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetVelocity(vel_x, (short)-vel_y, 0);
            }
            else if (cmd_name == "btnMoveBL")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetVelocity((short)-vel_x, vel_y, 0);
            }
            else if (cmd_name == "btnMoveBR")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetVelocity((short)-vel_x, (short)-vel_y, 0);
            }
            else if (cmd_name == "btnTurnLeft")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetVelocity(port.CarVel.v_linear_x, 0, get_vth(port.CarVel.v_linear_x));     //速度保持不变
            }
            else if (cmd_name == "btnTurnRight")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetVelocity(port.CarVel.v_linear_x, 0, (short)-get_vth(port.CarVel.v_linear_x));    //速度保持不变
            }
            else if (cmd_name == "btnSpin")
            {
                for (int i = 0; i < port_amount; i++)
                    ports[i].SetSpin((short)nmSpinVel.Value);
            }
        }

        private void MoveControl_Click(object sender, EventArgs e)
        {
            Button btn = (Button)sender;            
            LastCommand = sender;
            update_command(btn.Name);
            ActiveControl = panelCar;
        }

        /// <summary>
        /// 停止电机，并角度归零
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void btnSTOP2_Click(object sender, EventArgs e)
        {
            LastCommand = sender;
            port.SetVelocity(0, 0, 0);
            if (port2.Ready) port2.SetVelocity(0, 0, 0);
            ActiveControl = panelCar;
        }

        private void btnReset_Click(object sender, EventArgs e)
        {
            port.ResetWheels();
            if (port2.Ready) port2.ResetWheels();
            ActiveControl = panelCar;
        }

        private void btnFast_Click(object sender, EventArgs e)
        {
            if (WheelVel < 1.0)
            {
                WheelVel += 0.025;
                if (LastCommand != null) MoveControl_Click(LastCommand, e);
            }
            lbWheelVel.Text = WheelVel.ToString("F2");
        }

        private void btnSlow_Click(object sender, EventArgs e)
        {
            if(WheelVel > -1.0)
            {
                WheelVel -= 0.025;
                if (LastCommand != null) MoveControl_Click(LastCommand, e);
            }
            lbWheelVel.Text = WheelVel.ToString("F2");
        }

        private void btnRaiseStop_Click(object sender, EventArgs e)
        {
            port.Raise.down = 0x00;
            port.Raise.up = 0x00;
            port.Raise.pump = 0x00;
            port.SetRaise();
        }

        private void btnLiftUp_Click(object sender, EventArgs e)
        {
            port.Raise.down = 0x00;
            port.Raise.up = 0x01;
            port.Raise.pump = 0x01;
            port.SetRaise();
        }

        private void btnFallDown_Click(object sender, EventArgs e)
        {
            port.Raise.down = 0x01;
            port.Raise.up = 0x00;
            port.Raise.pump = 0x01;
            port.SetRaise();
        }

        private void btnMoveL_Click(object sender, EventArgs e)
        {
            LastCommand = sender;
            port.SetVelocity(0, Convert.ToInt16(WheelVel * 1000), 0);
            ActiveControl = panelCar;
        }

        private void btnMoveR_Click(object sender, EventArgs e)
        {
            LastCommand = sender;
            port.SetVelocity(0, Convert.ToInt16(-WheelVel * 1000), 0);
            ActiveControl = panelCar;
        }

        private void state_timer_Tick(object sender, EventArgs e)
        {
            Led.BackColor = port.Led? Color.LightGreen:Color.Gray;

            //不加Dispose会导致内存泄漏
            if (PicTeamed.Image != null) PicTeamed.Image.Dispose();
            if (PicTeamed2.Image != null) PicTeamed2.Image.Dispose();
            PicTeamed.Image = TeamImageList.Images[port.RobotState.team_info.enable>1?1: port.RobotState.team_info.enable];
            PicTeamed2.Image = TeamImageList.Images[port2.RobotState.team_info.enable > 1 ? 1 : port2.RobotState.team_info.enable];

            for (int i = 0; i < 4; i++)
            {
                Angles[i].Text = port.RobotState.angles[i].ToString();
                Heights[i].Height = (int)(port.RobotState.height[i]*20);
            }

            LbOdomTh.Text = (port.Odom.yaw/10.0).ToString();
            LbOdomX.Text = (port.Odom.x / 1000.0).ToString();
            LbOdomY.Text = (port.Odom.y / 1000.0).ToString();
            toolStripVersion.Text = port.Version;

        }

        private void BtnDisable2Car_Click(object sender, EventArgs e)
        {
            port.SetDoubleCar(false, DoubleCarPosition.Text, Convert.ToUInt32(CarCarDistance.Text));
            //二号端口对应后车
            if(port2.Ready) port2.SetDoubleCar(false, "Back", Convert.ToUInt32(CarCarDistance.Text));
        }

        private void BtnEnable2Car_Click(object sender, EventArgs e)
        {
            port.SetDoubleCar(true, DoubleCarPosition.Text, Convert.ToUInt32(CarCarDistance.Text));
            //二号端口对应后车
            if(port2.Ready) port2.SetDoubleCar(true, "Back", Convert.ToUInt32(CarCarDistance.Text));
        }

        private void BtnOpenPort2_Click(object sender, EventArgs e)
        {
            if (BtnOpenPort2.Text == "OpenPort2")
                port2.Begin(txtPort2.Text, 115200);
            else
                port2.Close();

            if (port2.Ready)
            {
                BtnOpenPort2.Text = "ClosePort2";
                port2.RequestRobotState();
            }
            else
            {
                BtnOpenPort2.Text = "OpenPort2";
            }
            ActiveControl = panelCar;
        }

        private void BtnResetOdom_Click(object sender, EventArgs e)
        {
            port.ClearOdom();
        }

        private void mainForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            port.Close();
        }
    }
}
